/*********************接线方式 
TCS3473x  Arduino_Uno 
  SDA         A4
  SCL         A5
  VIN         5V
  GND         GND
*************************/
#include<Servo.h>
Servo servo_pin_12;
#include <Wire.h>        //调用IIC库函数
#include "MH_TCS34725.h" //调用颜色识别传感器库函数
#ifdef __AVR__
#include <avr/power.h>
#endif

//颜色传感器不同通道值设置
MH_TCS34725 tcs = MH_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //设置颜色传感器采样周期50毫秒
//MH_TCS34725 tcs = MH_TCS34725(TCS34725_INTEGRATIONTIME_700MS, TCS34725_GAIN_1X);//设置颜色传感器采样周期700毫秒
//MH_TCS34725 tcs = MH_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_60X);//设置颜色传感器采样周期50毫秒（不推荐）
//MH_TCS34725 tcs = MH_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_16X);//设置颜色传感器采样周期50毫秒（不推荐）

void stop(int x,int y);
void start(int x,int y);
//设置直流电机停止
void stop(int x,int y){
  digitalWrite(x,LOW);
  digitalWrite(y,LOW);
}
//设置直流电机开始
void start(int x, int y){
  digitalWrite(x,HIGH);
  digitalWrite(y,LOW);
}
//设置电机降速
void lower(int x,int y){
  analogWrite(x, 50); 
  analogWrite(y, 50);
}
//设置电机加速
void rise(int x,int y){
  analogWrite(x, 150); 
  analogWrite(y, 150);
}


void setup() {
  pinMode(14,INPUT);
  pinMode(16,INPUT);
  pinMode(18,INPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(10,OUTPUT);
  //设置左轮转速
//  analogWrite(5, 100); 
//  analogWrite(6, 100);
  //设置右轮转速
  analogWrite(9, 50); 
  analogWrite(10, 50);
  //灰度传感器检测到深色标记时，电机转动
//  if(!(digitalRead(14)&&digitalRead(16)&&digitalRead(18)))
//  {
//  start(5,6); //左车轮开始转动
//  start(9,10); //右车轮开始转动
//  }
  servo_pin_12.attach(12);
  servo_pin_12.write(90);
  Serial.begin(115200);  //开启串口，并设置串口波特率位115200
  Serial.println("Color View Test!"); //串口打印：Color View Test!
  //检测是否有颜色传感器模块
  if (tcs.begin()) {                 //如果检测到颜色传感器模块
    Serial.println("Found sensor");  //串口打印 Found sensor
  } else {                           //如果没有检测到颜色传感器模块
    Serial.println("No TCS34725 found ... check your connections");//串口打印：没有找到颜色识别传感器模块
    while (1); // halt! //程序陷入死循环
  }
}

//串口打印Red、Green、Blue三色值
void loop() { 

  digitalWrite(5,HIGH);
  digitalWrite(6,LOW);
  analogWrite(5, 100); 
  analogWrite(6, 100);
//  stop(5,6);
//  start(9,10);
  
//  if(!(digitalRead(14))){
//    rise(5,6);
//    lower(9,10);
//  }
//  else if(digitalRead(18)){
//    rise(9,10);
//    lower(5,6);
//  }

  
  uint16_t clear, red, green, blue; //分别定义用于存储红、绿、蓝三色值变量
  tcs.getRGBC(&red, &green, &blue, &clear); //将原始R/G/B值转换为色温（以度为单位）
  tcs.lock();  //禁用中断（可省略）



     

    

  //颜色识别
  uint32_t sum = clear;           //===========
  float r, g, b;                  // 计算红
  r = red; r /= sum;              // 绿、蓝
  g = green; g /= sum;            // 三色数
  b = blue; b /= sum;             // 值
  r *= 256; g *= 256; b *= 256;   //===========
  
  if((r>0)&&(g>0)&&(b>0)&&(g<r)&&(b<r))
  {
    servo_pin_12.write(0);
  }
  else   if((r>0)&&(g>0)&&(b>0)&&(r<g)&&(b<g))
  {
    servo_pin_12.write(90);
  }
  else   if((r>0)&&(g>0)&&(b>0)&&(g<b)&&(r  <b))
  {
    servo_pin_12.write(180);
  }

}
